#include "esp_camera.h"
#include <WiFi.h>

// ===========================
// Camera server setup (simplified)
// ===========================
#include "esp_http_server.h"
// ===========================
// Camera pins for AI Thinker ESP32-CAM
// ===========================
#define PWDN_GPIO_NUM     32
#define RESET_GPIO_NUM    -1
#define XCLK_GPIO_NUM      0
#define SIOD_GPIO_NUM     26
#define SIOC_GPIO_NUM     27
#define Y9_GPIO_NUM       35
#define Y8_GPIO_NUM       34
#define Y7_GPIO_NUM       39
#define Y6_GPIO_NUM       36
#define Y5_GPIO_NUM       21
#define Y4_GPIO_NUM       19
#define Y3_GPIO_NUM       18
#define Y2_GPIO_NUM        5
#define VSYNC_GPIO_NUM    25
#define HREF_GPIO_NUM     23
#define PCLK_GPIO_NUM     22


// ===========================
// Motor control pins (单电机)
// ===========================
#define MOTOR_IN1 14
#define MOTOR_IN2 15
#define ENA 13

// ===========================
// WiFi credentials
// ===========================
const char *ssid = "ChinaNet-6x8c";
const char *password = "8zeymm8c";
// ===========================
// Function declarations
// ===========================
void startCameraServer();
void motorForward();
void motorBackward();
void motorStop();

#define PART_BOUNDARY "123456789000000000000987654321"
static const char *_STREAM_CONTENT_TYPE = "multipart/x-mixed-replace;boundary=" PART_BOUNDARY;
static const char *_STREAM_BOUNDARY = "\r\n--" PART_BOUNDARY "\r\n";
static const char *_STREAM_PART = "Content-Type: image/jpeg\r\nContent-Length: %u\r\nX-Timestamp: %d.%06d\r\n\r\n";

httpd_handle_t stream_httpd = NULL;
httpd_handle_t camera_httpd = NULL;

void setup() {
  Serial.begin(115200);
  Serial.setDebugOutput(false);  // Disable debug output to save memory
  Serial.println();
  Serial.println("Starting ESP32-CAM...");

    // 初始化电机控制
  setupMotor();

  // Camera configuration
  camera_config_t config;
  config.ledc_channel = LEDC_CHANNEL_0;
  config.ledc_timer = LEDC_TIMER_0;
  config.pin_d0 = Y2_GPIO_NUM;
  config.pin_d1 = Y3_GPIO_NUM;
  config.pin_d2 = Y4_GPIO_NUM;
  config.pin_d3 = Y5_GPIO_NUM;
  config.pin_d4 = Y6_GPIO_NUM;
  config.pin_d5 = Y7_GPIO_NUM;
  config.pin_d6 = Y8_GPIO_NUM;
  config.pin_d7 = Y9_GPIO_NUM;
  config.pin_xclk = XCLK_GPIO_NUM;
  config.pin_pclk = PCLK_GPIO_NUM;
  config.pin_vsync = VSYNC_GPIO_NUM;
  config.pin_href = HREF_GPIO_NUM;
  config.pin_sccb_sda = SIOD_GPIO_NUM;
  config.pin_sccb_scl = SIOC_GPIO_NUM;
  config.pin_pwdn = PWDN_GPIO_NUM;
  config.pin_reset = RESET_GPIO_NUM;
  
  // Reduced configuration to prevent memory overflow
  config.xclk_freq_hz = 10000000;    // 10MHz clock (reduced from 20MHz)
  config.frame_size = FRAMESIZE_VGA; // VGA resolution (640x480)
  config.pixel_format = PIXFORMAT_JPEG;
  config.jpeg_quality = 15;          // Medium quality
  config.fb_count = 1;               // Single frame buffer
  config.grab_mode = CAMERA_GRAB_WHEN_EMPTY;

  // Adjust based on PSRAM availability
  if (psramFound()) {
    Serial.println("PSRAM detected - using optimized settings");
    config.fb_location = CAMERA_FB_IN_PSRAM;
    config.jpeg_quality = 23;
    config.fb_count = 1;
  } else {
    Serial.println("No PSRAM - using minimal settings");
    config.fb_location = CAMERA_FB_IN_DRAM;
    config.frame_size = FRAMESIZE_QVGA; // Further reduce to QVGA
    config.jpeg_quality = 18;
  }

  // Initialize camera
  esp_err_t err = esp_camera_init(&config);
  if (err != ESP_OK) {
    Serial.printf("Camera init failed with error 0x%x\n", err);
    Serial.println("Trying with lowest possible configuration...");
    
    // Try with absolute minimum configuration
    config.frame_size = FRAMESIZE_QVGA;
    config.jpeg_quality = 20;
    config.xclk_freq_hz = 5000000;
    config.fb_count = 1;
    
    err = esp_camera_init(&config);
    
    if (err != ESP_OK) {
      Serial.printf("Camera init failed again with error 0x%x\n", err);
      Serial.println("Restarting in 3 seconds...");
      delay(3000);
      ESP.restart();
      return;
    }
  }

  Serial.println("Camera initialized successfully");

  // Adjust camera settings for better performance
  sensor_t *s = esp_camera_sensor_get();
  if (s->id.PID == OV2640_PID) {
    // Common settings for OV2640
    s->set_vflip(s, 1);        // Vertical flip
    s->set_brightness(s, 1);   // Increase brightness
    s->set_saturation(s, -2);  // Reduce saturation
    
    // Set to lower resolution for streaming
    s->set_framesize(s, FRAMESIZE_QVGA);
  }

  // Connect to WiFi
  WiFi.begin(ssid, password);
  WiFi.setSleep(false);

  Serial.print("Connecting to WiFi");
  int attempts = 0;
  while (WiFi.status() != WL_CONNECTED && attempts < 25) {
    delay(500);
    Serial.print(".");
    attempts++;
  }

  if (WiFi.status() != WL_CONNECTED) {
    Serial.println("\nWiFi connection failed!");
    Serial.println("Please check your credentials and try again.");
    delay(3000);
    ESP.restart();
    return;
  }

  Serial.println("\nWiFi connected successfully!");
  Serial.print("IP address: ");
  Serial.println(WiFi.localIP());

  // Start camera server
  startCameraServer();
  Serial.println("Camera server started");
  Serial.println("Open http://" + WiFi.localIP().toString() + " in your browser");
}

void loop() {
  // Monitor system health
  static unsigned long lastCheck = millis();
  if (millis() - lastCheck > 30000) {
    lastCheck = millis();
    
    // Check free memory
    uint32_t freeHeap = ESP.getFreeHeap();
    Serial.printf("Free memory: %d bytes\n", freeHeap);
    
    // Check WiFi connection
    if (WiFi.status() != WL_CONNECTED) {
      Serial.println("WiFi disconnected! Attempting to reconnect...");
      WiFi.reconnect();
    }
    
    // Restart if memory gets too low
    if (freeHeap < 15000) {
      Serial.println("Low memory detected, restarting...");
      delay(1000);
      ESP.restart();
    }
  }
  
  delay(10000);
}

// 初始化电机控制引脚
void setupMotor() {
  pinMode(MOTOR_IN1, OUTPUT);
  pinMode(MOTOR_IN2, OUTPUT);
  analogWrite(ENA, 180); // 速度控制（0-255）
  motorStop();
  Serial.println("Motor pins initialized");
}
// 电机控制函数
void motorForward() {
  digitalWrite(MOTOR_IN1, HIGH);
  digitalWrite(MOTOR_IN2, LOW);
  analogWrite(ENA, 180);
  Serial.println("Motor: Forward");
}

void motorBackward() {
  digitalWrite(MOTOR_IN1, LOW);
  digitalWrite(MOTOR_IN2, HIGH);
  analogWrite(ENA, 180);
  Serial.println("Motor: Backward");
}

void motorStop() {
  digitalWrite(MOTOR_IN1, LOW);
  digitalWrite(MOTOR_IN2, LOW);
  analogWrite(ENA, 0);
  Serial.println("Motor: Stop");
}

// 电机控制处理函数
static esp_err_t control_handler(httpd_req_t *req) {
  char buf[100];
  int ret, remaining = req->content_len;
  
  // 读取请求数据
  int total_received = 0;
  while (remaining > 0) {
    ret = httpd_req_recv(req, buf + total_received, min(remaining, (int)sizeof(buf) - total_received - 1));
    if (ret <= 0) {
      return ESP_FAIL;
    }
    total_received += ret;
    remaining -= ret;
  }
  buf[total_received] = '\0';
  
  // 解析控制命令
  String command = String(buf);
  Serial.println("Received command: " + command);
  
  if (command == "forward") {
    motorForward();
  } else if (command == "backward") {
    motorBackward();
  } else if (command == "stop") {
    motorStop();
  }
  
  httpd_resp_set_type(req, "text/plain");
  return httpd_resp_send(req, "OK", 2);
}

// ===================== HTTP服务器处理函数 =====================
static esp_err_t stream_handler(httpd_req_t *req) {
  camera_fb_t *fb = NULL;
  struct timeval _timestamp;
  esp_err_t res = ESP_OK;
  size_t _jpg_buf_len = 0;
  uint8_t *_jpg_buf = NULL;
  char *part_buf[128];

  static int64_t last_frame = 0;
  if (!last_frame) {
    last_frame = esp_timer_get_time();
  }

  res = httpd_resp_set_type(req, _STREAM_CONTENT_TYPE);
  if (res != ESP_OK) {
    return res;
  }

  httpd_resp_set_hdr(req, "Access-Control-Allow-Origin", "*");
  httpd_resp_set_hdr(req, "X-Framerate", "60");

  while (true) {
    fb = esp_camera_fb_get();
    if (!fb) {
      log_e("Camera capture failed");
      res = ESP_FAIL;
    } else {
      _timestamp.tv_sec = fb->timestamp.tv_sec;
      _timestamp.tv_usec = fb->timestamp.tv_usec;
      if (fb->format != PIXFORMAT_JPEG) {
        bool jpeg_converted = frame2jpg(fb, 80, &_jpg_buf, &_jpg_buf_len);
        esp_camera_fb_return(fb);
        fb = NULL;
        if (!jpeg_converted) {
          log_e("JPEG compression failed");
          res = ESP_FAIL;
        }
      } else {
        _jpg_buf_len = fb->len;
        _jpg_buf = fb->buf;
      }
    }
    if (res == ESP_OK) {
      res = httpd_resp_send_chunk(req, _STREAM_BOUNDARY, strlen(_STREAM_BOUNDARY));
    }
    if (res == ESP_OK) {
      size_t hlen = snprintf((char *)part_buf, 128, _STREAM_PART, _jpg_buf_len, _timestamp.tv_sec, _timestamp.tv_usec);
      res = httpd_resp_send_chunk(req, (const char *)part_buf, hlen);
    }
    if (res == ESP_OK) {
      res = httpd_resp_send_chunk(req, (const char *)_jpg_buf, _jpg_buf_len);
    }
    if (fb) {
      esp_camera_fb_return(fb);
      fb = NULL;
      _jpg_buf = NULL;
    } else if (_jpg_buf) {
      free(_jpg_buf);
      _jpg_buf = NULL;
    }
    if (res != ESP_OK) {
      log_e("Send frame failed");
      break;
    }
    int64_t fr_end = esp_timer_get_time();

    int64_t frame_time = fr_end - last_frame;
    last_frame = fr_end;

    frame_time /= 1000;
    log_i(
      "MJPG: %uB %ums (%.1ffps), AVG: %ums (%.1ffps)", (uint32_t)(_jpg_buf_len), (uint32_t)frame_time, 1000.0 / (uint32_t)frame_time, avg_frame_time,
      1000.0 / avg_frame_time
    );
  }
  return res;
}


static esp_err_t index_handler(httpd_req_t *req) {
  const char* html = R"rawliteral(
<!DOCTYPE html>
<html>
<head>
    <title>ESP32-CAM 单电机控制</title>
    <meta name="viewport" content="width=device-width, initial-scale=1" charset="UTF-8">
    <style>
        body { margin: 0; padding: 20px; background: #2c3e50; color: white; text-align: center; font-family: Arial, sans-serif; }
        .container { max-width: 800px; margin: 0 auto; }
        .control-panel { background: #34495e; padding: 20px; border-radius: 10px; margin-bottom: 20px; }
        .btn { padding: 15px 25px; margin: 10px; border: none; border-radius: 8px; font-size: 16px; cursor: pointer; }
        .forward { background: #27ae60; color: white; }
        .backward { background: #e74c3c; color: white; }
        .stop { background: #7f8c8d; color: white; }
        .video-container { background: #34495e; padding: 15px; border-radius: 10px; }
        .video-stream { width: 100%; max-width: 320px; border: 2px solid #7f8c8d; border-radius: 5px; }
        .status { margin-top: 10px; color: #bdc3c7; }
    </style>
</head>
<body>
    <div class="container">
        <div class="control-panel">
            <div>
                <button class="btn forward" onmousedown="controlMotor('forward')" ontouchstart="controlMotor('forward')" onmouseup="controlMotor('stop')" ontouchend="controlMotor('stop')">前进</button>
            </div>
            <div>
                <button class="btn stop" onclick="controlMotor('stop')">停止</button>
            </div>
            <div>
                <button class="btn backward" onmousedown="controlMotor('backward')" ontouchstart="controlMotor('backward')" onmouseup="controlMotor('stop')" ontouchend="controlMotor('stop')">后退</button>
            </div>
            <div id="status" class="status">就绪</div>
        </div>

        <div class="video-container">
            <img src="http://192.168.1.39:81/stream" class="video-stream" 
                 onload="document.getElementById('videoStatus').textContent='视频流连接正常'"
                 onerror="document.getElementById('videoStatus').textContent='视频流连接失败'">
            <div id="videoStatus" class="status">正在连接视频流...</div>
        </div>
    </div>

    <script>
        function controlMotor(direction) {
            const status = document.getElementById('status');
            status.textContent = '控制: ' + direction;
            
            fetch('/control', {
                method: 'POST',
                body: direction,
                headers: {
                    'Content-Type': 'text/plain'
                }
            })
            .then(response => response.text())
            .then(data => {
                console.log('控制成功:', direction);
            })
            .catch(error => {
                status.textContent = '控制失败';
                console.error('错误:', error);
            });
        }

        // 自动重连视频流
        setInterval(() => {
            const img = document.querySelector('.video-stream');
            if (img && img.complete && img.naturalHeight === 0) {
                img.src = img.src.split('?')[0] + '?t=' + Date.now();
            }
        }, 5000);
    </script>
</body>
</html>
)rawliteral";
  
  httpd_resp_set_type(req, "text/html");
  return httpd_resp_send(req, html, strlen(html));
}

void startCameraServer() {
  httpd_config_t config = HTTPD_DEFAULT_CONFIG();
  config.server_port = 80;
  // config.stack_size = 4096;  // 堆栈大小
  config.max_uri_handlers = 16;
  // config.lru_purge_enable = true;

  httpd_uri_t index_uri = {
    .uri = "/",
    .method = HTTP_GET,
    .handler = index_handler,
    .user_ctx = NULL
  };

  httpd_uri_t stream_uri = {
    .uri = "/stream",
    .method = HTTP_GET,
    .handler = stream_handler,
    .user_ctx = NULL
  };

    httpd_uri_t control_uri = {
    .uri = "/control",
    .method = HTTP_POST,
    .handler = control_handler,
    .user_ctx = NULL
  };
  if (httpd_start(&camera_httpd, &config) == ESP_OK) {
    httpd_register_uri_handler(camera_httpd, &index_uri);
    httpd_register_uri_handler(camera_httpd, &control_uri);
    Serial.println("camera_httpd服务器启动成功");
  } else {
    Serial.println("camera_httpd服务器启动失败");
  }

  config.server_port += 1;
  config.ctrl_port += 1;
  if (httpd_start(&stream_httpd, &config) == ESP_OK) {
    httpd_register_uri_handler(stream_httpd, &stream_uri);
    Serial.println("stream_httpd服务器启动成功");
  } else {
    Serial.println("stream_httpd服务器启动失败");
  }

}
  